clear all; close all;
Len = 1000;
p = 3; 
q = 3;
dt = 0.01;
Ak = [1 dt 0;
    0 1 dt;
    0 0 1];
Ck = randn(q,p);
xk = [0;0;1];
wk = randn(p, Len) * 0.01;
vk = randn(q, Len) * 2;
Q = wk * wk' / Len;
R = vk * vk' / Len;

for ii = 1 : Len
    xk(:,ii+1) = Ak * xk(:, ii) + wk(:, ii);
    yk(:, ii) = Ck * xk(:, ii+1) + vk(:, ii);
end



%% KF 
xe = randn(p, Len+1);
Pkk = eye(p);
for ii = 1 : Len
    Err_kf(ii) = norm(xe(:,ii) - xk(:, ii));
    Pkk = Ak*Pkk*Ak'+Q;
    Hk=Pkk*Ck'/(Ck*Pkk*Ck'+R);
    xe(:,ii+1) = Ak*xe(:,ii)+Hk*(yk(:,ii)-Ck*Ak*xe(:,ii));
    Pkk = (eye(p)-Hk*Ck)*Pkk;

    
end
figure, hold on

plot(xk(1,:),"r")
plot(Err_kf,"g")
plot(xe(1,:),"b")
legend('xk','err','xke');

